#include "ThreadServoRate.h"

ThreadServoRate::ThreadServoRate( int nPin,
                                  unsigned long nMaxServoSpeedInMicrosPerDegree,
                                  unsigned int nInitialPosition,
                                  unsigned int nInitialRateInDegPerSec )
{
  this->nCurPosition = nInitialPosition;
  this->nTargetPosition = nCurPosition;
  this->nMaxServoSpeedInMicrosPerDegree = nMaxServoSpeedInMicrosPerDegree;
  this->nMoveRateInDegPerSec = nInitialRateInDegPerSec;
  this->nLastMoveTimestampInMicros = micros();
  this->obj.attach( nPin );
  this->obj.write( nInitialPosition );
}

ThreadServoRate::~ThreadServoRate()
{
}

unsigned int ThreadServoRate::getPosition()
{
  unsigned int nReturn = 0;
  nReturn = nCurPosition;

  return nReturn;
}

void ThreadServoRate::setPosition( int nTargetPosition,
                                   int nMoveRateInDegPerSec )
{
  this->nMoveRateInDegPerSec = nMoveRateInDegPerSec;
  this->nTargetPosition = nTargetPosition;
}

boolean ThreadServoRate::isMoving()
{
  return ( nTargetPosition != nCurPosition );
}

// Local variables problematic in protothreads so I made this macro to compute next position.
#define __SERVO_POS_NEXT(nDiffFromTarget) (nCurPosition) + ( ((nDiffFromTarget) > 0) ? 1 : -1 )
#define __SERVO_DELAY_MICROS() ( ( (nMoveRateInDegPerSec) > 0 ) ? 1000000 / (nMoveRateInDegPerSec) : 0 )
#define __SERVO_ELAPSED_TIME() ( micros() - nLastMoveTimestampInMicros )
#define __SERVO_DELAY_MICROS_LESS_ELAPSED_TIME() ( (__SERVO_DELAY_MICROS() > __SERVO_ELAPSED_TIME()) ? (__SERVO_DELAY_MICROS() - __SERVO_ELAPSED_TIME()) : 0  )

bool ThreadServoRate::run()
{
  threadMicroDelay.run();

  int nDiff = (int) nTargetPosition - (int) nCurPosition;

  PT_BEGIN()
  ;

  if ( nDiff != 0 )
  {
    // tell servo to move 1 degree to next position
    nLastMoveTimestampInMicros = micros();
    obj.write( __SERVO_POS_NEXT( nDiff) );

    // wait for the servo to move 1 degree (so, wait nMaxServoSpeedInMicrosPerDegree microseconds)
    threadMicroDelay.setDelay( nMaxServoSpeedInMicrosPerDegree );
    PT_SPAWN( threadMicroDelay );

    // servo has had time to get to next degree, so update the current position in state structure
    nCurPosition = __SERVO_POS_NEXT( nDiff);

    // delay some to implement the move rate.  So this is (1000000 / nMoveRateInDegPerSec) , but we
    // have to subtract off the time is has taken us to move 1 degree, which is
    //  micros() - nLastMoveTimestampInMicros.  And we only need to do this if we are not at
    // the target position and the move rate is > 0.  Move rate == 0 means full speed (no delay).

    if ( ( nMoveRateInDegPerSec > 0 ) && ( nTargetPosition != nCurPosition ) )
    {
      //      lcd.setCursor(10,1);
      //      lcd.print(__SERVO_DELAY_MICROS_LESS_ELAPSED_TIME(tsServo));

      threadMicroDelay.setDelay( __SERVO_DELAY_MICROS_LESS_ELAPSED_TIME() );
      PT_SPAWN( threadMicroDelay );
      //      delay(__SERVO_DELAY_MICROS_LESS_ELAPSED_TIME(tsServo) / 1000);
      //      delayMicroseconds(__SERVO_DELAY_MICROS_LESS_ELAPSED_TIME(tsServo) % 1000);
    }
  }

PT_END()
;
return false;
}

